#!/usr/bin/env python
#-*-coding:utf-8-*-
import rospy
import os
import sys
import numpy as np
import uuid, datetime, time
from nav_msgs.msg import Path,Odometry


dis_check_time_=0
lst_loc_=np.zeros((2),dtype=np.float32)
car_stop_=False
stop_time_=0
count_=0
saving_=False

def gps_odom_call(msg):
    # print("get new loc")
    global dis_check_time_, lst_loc_, car_stop_, count_
    loc=np.zeros((2))
    loc[0]=msg.pose.pose.position.x
    loc[1]=msg.pose.pose.position.y
    # print(np.around(loc, 2))
    # loc = np.around(loc, 2)
    if (msg.header.stamp.to_sec()-dis_check_time_)>3:
        dis_check_time_=msg.header.stamp.to_sec()
        check_stop(loc)
        
def check_stop(loc):
    global dis_check_time_, lst_loc_, car_stop_, stop_time_, count_, saving_
    is_stop=False
    # debug code
    # print("count", count_)
    # if (count_>10 and count_<20) or (count_>30 and count_<40) or (count_>50):
    #     loc = lst_loc_
    # count_+=1
    if np.all(lst_loc_==0):
        lst_loc_ = loc
        return

    diff = loc - lst_loc_
    dis = np.linalg.norm(diff)

    print("loc", loc)
    print("last loc", lst_loc_)
    print("dis", dis)
    if dis < 0.5:
        if (not is_stop):
            stop_time_=rospy.Time.now().to_sec()
        is_stop=True
    
    # print("last loc", lst_loc_)
    # print("now loc", loc)
    lst_loc_=loc
    if (is_stop and saving_):
        saving_=False
        print("car stopped")

    elif ((not is_stop) and (not saving_)):
        saving_=True
        print("car moved")

    print("now car stop", is_stop)
    print("-------------------------")

if __name__ == "__main__":
    # print("car moved")
    # print(os.path.abspath(__file__))
    # path = os.path.split(os.path.abspath(__file__))[0]
    # os.system("/bin/bash " + path + "/save_sync_sensor_data.sh")
    
    # print("car stopped")
    # result = os.popen("rosnode list")
    # nodelets = result.read().split()
    # nodelets = [x for x in nodelets if "save_" in x]
    # for nodelet in nodelets:
    #     command = "rosnode kill " + nodelet
    #     print(command)
    #     os.system(command)

    print("init node")
    rospy.init_node("auto_start", anonymous=True)
    rospy.Subscriber("/gps_odom", Odometry, gps_odom_call)
    rospy.spin()
